% compute the outputs from mean joint angles.

addpath_output()
% name list 9 person
name  = {'Charles','Fred','Martin','Ram','Selina',... % 1st experiment
    'lily','po','ryan','vic'};  % 2nd experiment

% CONSTANT
% the number of subjects from the 1st experiment
exp1st = 5;
% the index of subjects use the right leg as swing leg during the step
rIndex = [6,7,9];
% experimental sampling frequency
fs = 480;
model = 'NAO';
% model = 'AMBER';
% model = 'HM';

time_rec = zeros(9,1);
for index = 1:length(name) % 5
  
    % get human data, with breaking pts for each step
    humanData = getData(name{1,index});
    
%     sum_weight  = sum_weight+humanData.weight; % for average weight
    % breaking pts for each step
    StartPt = humanData.sns_brk(2);
    EndPt = humanData.sns_brk(3);
    
    % position
    if index <= exp1st
        [xpos,ypos,zpos] = getPosCartesian(humanData); % subjects from experiment 1st
    else
        [xpos,ypos,zpos] = getPosCartesianNew(humanData); % subjects from experiment 2st
    end

    %%%%%%%%%%%  time  %%%%%%%%%%%%%%
    % scale time
    % from [c:d]
    ns_time_pre = (0:length(StartPt:EndPt)-1)'/fs;
    ns_ret = ns_time_pre;
    xpara_time = 1/ns_ret(end); % simply sacled time to 1
    ns_time = ns_ret*xpara_time;
    %%%% record time %%%%%%%%
    time_rec(index) = ns_ret(end);
    
   
    
    %%%%%%%%%%%% calculate human joint angles %%%%%%%%%%%%%%%%%
    if ismember(index,rIndex)
        angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,2);
    else
        angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,1);
    end
    
     % 1. stance ankle angle
    [ave_dataSA,ave_timeSA] = BeamData(ns_time,angle.sa);
    ave_dataSA_s(index,:) = ave_dataSA;     
    % 2. stance knee
    [ave_dataSK,ave_timeSK] = BeamData(ns_time,angle.sknee);
    ave_dataSK_s(index,:) = ave_dataSK;
%     plot(ave_dataSK); hold on;
    % 3. theta3
    [ave_dataT3,ave_timeT4] = BeamData(ns_time,angle.theta3);
    ave_dataT3_s(index,:) = ave_dataT3;
    % 4. theta4
    [ave_dataT4,ave_timeT4] = BeamData(ns_time,angle.theta4);
    ave_dataT4_s(index,:) = ave_dataT4;
    % 5. non-stance knee
    [ave_dataNSK,ave_timeNSK] = BeamData(ns_time,angle.nsknee);
    ave_dataNSK_s(index,:) = ave_dataNSK;
%         plot(ave_dataNSK); hold on;

end

    % 1. stance ankle angle
    [x_meanSA,upperBSA,lowerBSA]=meanValue(ave_dataSA_s);

    % 2. stance knee
     [x_meanSK,upperBSK,lowerBSK] = meanValue(ave_dataSK_s);

    % 3. theta3
    [x_meanT3,upperBT3,lowerBT3]=meanValue(ave_dataT3_s);    
       
    % 4. theta4
    [x_meanT4,upperBT4,lowerBT4]=meanValue(ave_dataT4_s);
    
    % 5. non-stance knee
   [x_meanNSK,upperBNSK,lowerBNSK] = meanValue(ave_dataNSK_s);
%%
   q = [x_meanSA; x_meanSK; x_meanT3; x_meanT4; x_meanNSK];
   outputs = cal_robot_para_18(q,model);
   
   qu = [upperBSA; upperBSK; upperBT3; upperBT4; upperBNSK];
   upperB = cal_robot_para_18(qu,model);
   
   
   ql = [lowerBSA; lowerBSK; lowerBT3; lowerBT4; lowerBNSK];
   lowerB = cal_robot_para_18(ql,model);
   
   %%
   
upperBHP = upperB.hippos;
lowerBHP = lowerB.hippos;
x_meanHP = outputs.hippos;
% linearized hip position
% ave_timeHPL;
upperBHPL = upperB.delta_hippos;
lowerBHPL = lowerB.delta_hippos;
x_meanHPL = outputs.delta_hippos;
% non-stance slope
% ave_timeNSslopeL,upperBNSL,lowerBNSL,x_meanNSL
% ave_timeNSslope = ave_timeNSslope;
upperBNSL = upperB.nsslope;
lowerBNSL = lowerB.nsslope;
x_meanNSL = outputs.nsslope;
% linearized non-stance slope
% ave_timeNSslopeL = ave_timeNSslopeL;
upperBLNS = upperB.delta_nsslope;
lowerBLNS = lowerB.delta_nsslope;
x_meanLNS = outputs.delta_nsslope;
% stance COM slope
upperBSCOM = upperB.stCOM;
lowerBSCOM = lowerB.stCOM;
x_meanSCOM = outputs.stCOM;
% linearized stance COM slope
upperBSCOML = upperB.delta_stCOM;
lowerBSCOML = lowerB.delta_stCOM;
x_meanSCOML = outputs.delta_stCOM;


%% plot
    meanValuePlot(ave_timeSA,upperBHP,lowerBHP,x_meanHP,'HipPos.eps',...
        'Position(m)',1);
   
    meanValuePlot(ave_timeSA,upperBHPL,lowerBHPL,x_meanHPL,'HipPosLinear.eps',...
        'Position(m)',2);

    meanValuePlot(ave_timeSA,upperBNSL,lowerBNSL,x_meanNSL,'nsSlope.eps',...
        'Slope',3)

    meanValuePlot(ave_timeSA,upperBLNS,lowerBLNS,x_meanLNS,'nsSlopeLiear.eps',...
        'Slope',4)

    meanValuePlot(ave_timeSA,upperBSCOM,lowerBSCOM,x_meanSCOM,'stCOM.eps',...
        'Slope',9);

        meanValuePlot(ave_timeSA,upperBSCOML,lowerBSCOML,x_meanSCOML,'stCOMLinear.eps',...
        'Slope',10);